#include "devcan.hpp"
#include "sysapi.hpp"

#define LOG_TAG "driver_devcan"
#define LOG_LVL LOG_LVL_DBG
#include <ulog.h> 


/* robot_can_dev functions define start */
/**
 * @brief Construct a new robot can dev::robot can dev object
 * @param  b                My Param doc
 */
robot_can_dev::robot_can_dev(robot_can_bus* b): bus(b)
{
    rt_sem_init(&r_sem, "bscan", 0, RT_IPC_FLAG_FIFO);
    for(int i=0; i<id_list_size; i++)
    {
        id_list[i] = 0;
    }
    id_list_len = 0;
    device_on_bus.data = this;
};

/**
 * @brief Destroy the robot can dev::robot can dev object
 */
robot_can_dev::~robot_can_dev()
{
    rt_sem_detach(&r_sem);
    robot_list::remove(&this->bus->head, &this->device_on_bus);
};

/**
 * @brief 
 * @param  id               My Param doc
 */
void robot_can_dev::add_id(uint32_t id)
{
    id_list[id_list_len] = id;
	id_list_len++;
}

/**
 * @brief 
 * @param  pos             
 * @param  buffer           
 * @param  size           
 * @return int 
 */
int robot_can_dev::read(int pos, void *buffer,int size)
{
    int ret;
    if(rt_sem_trytake(&r_sem) != RT_EOK)
    {
        ret = 0;
    }
    else
    {
        if(size < sizeof(rt_can_msg))
        {
            ret =  0;
        }
        else
        {
            can_msg temp;
            temp = msg.pop();
            rt_memcpy(buffer, temp.get_data_p(), sizeof(rt_can_msg));
            ret = sizeof(rt_can_msg); 
        }
    }
	return ret;
}

/**
 * @brief 
 * @param  pos              My Param doc
 * @param  buffer           My Param doc
 * @param  size             My Param doc
 * @return int 
 */
int robot_can_dev::read_block(int pos, void *buffer,int size)
{
    rt_sem_take(&r_sem, RT_WAITING_FOREVER);
    can_msg temp;
    temp = msg.pop();
    rt_memcpy(buffer, temp.get_data_p(), sizeof(rt_can_msg));
	return 0;
}

/**
 * @brief 
 * @param  pos              My Param doc
 * @param  buffer           My Param doc
 * @param  size             My Param doc
 * @return int 
 */
int robot_can_dev::write(int pos, const void *buffer,int size)
{
    can_msg temp((struct rt_can_msg*)buffer);
    bus->msg_f.push(temp);
    rt_sem_release(&bus->s_sem);
	return 0;
}

void robot_can_dev::read_clear()
{
    while(rt_sem_trytake(&r_sem) == RT_EOK);
    msg.clear();
}

/**
 * @brief 
 */
void robot_can_dev::open()
{
    robot_list::insert(&this->bus->head, &this->device_on_bus);
}
/* robot_can_dev functions define end */